#include "RRT.h"

int main(void)
{
	//ACSR = (1 << ACD);
	init_utilities();
	init_USART();
	init_ADC();
	init_Timer0();
	
	delay_ms(2500);
	
	start_AI();
	
	
}

ISR(TIMER0_COMPB_vect){
	toggle_led();
	//delay_ms(500);
}

void start_AI() {
	
	int state = 0;
	
	while(1) {
		switch(state){
			case 0: {
				// We are standing still and no obstacle is in front of us. We start moving!
				//move_forward(100);
				state = 1;
				break;
			}
			case 1: {
				state = 0;
				break;
				// We are moving and are continuously checking for obstacles
				if(is_obstacle_IFOM()) {
					stop_moving();
					state = 2;
				}
				// We check for a white marker on the floor
				if(is_marker_OTF()) {
					stop_moving();
					print_text("Its sticky here");
				}
				break;
			}
			case 2: {
				// We are standing still and are searching for a
				// direction to continue/start our travel!
				uint8_t dDegree = 360/8;
				for(uint8_t i = 0; i<8; i++) {
					print_number(i);
					rotate_right(dDegree);
					
					// Check if we can move now
					if(!is_obstacle_IFOM()) {
						state = 0;
						break;
					}
				}
				if(state!=0){
					// Error we are trapped!
					state = 3;
				}
				break;
			}
			case 3: {
				// Error state
				print_text("I am trapped :(");
				break;
			}
		}		
	}	
}
